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ABSTRACT 


Many  underwater  vehicles  have  been  designed  to  follow  a  straight  path  using  linear 
approximations  about  that  path.  Tracking  a  dynamic  path  of  arbitrary  but  continuous 
curvature  may  often  be  desired.  This  will  require  a  nonlinear  controller  with  enhanced 
robustness  properties.  One  point  of  this  thesis  is  to  show  how  nonlinear  control  using 
sliding  modes  may  be  applied  to  follow  a  dynamic  path. 

In  a  mine  warfare  setting  using  Autonomous  Underwater  Vehicles  (AUVs),  reflexive 
maneuvers  will  be  required  for  mine  avoidance.  This  thesis  presents  one  way  in  which 
paths  for  mine  avoidance  maneuvers  may  be  generated  automatically  and  used  as  inputs  to 
the  nonlinear  tracking  control  system  of  the  vehicle.  It  has  been  shown  through  simulation 
that  a  random  minefleld  can  be  traversed  by  an  AUV  while  localizing  and  avoiding 
detected  mines  using  these  control  concepts. 
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INTRODUCTION 


A.  BACKGROUND  AND  MOTIVATION 

Autonomous  Underwater  Vehicles  (AUVs)  are  unmanned  and  independent  craft  with 
respect  to  power  and  control.  Unlike  Remotely  Operated  Vehicles  (ROVs)  which  require 
tethered  cables  for  external  power  and  control,  AUVs  require  no  extenud  interface.  AUVs 
are  receiving  interest  by  both  naval  and  civilian  activities  operating  in  the  ocean 
environment.  Their  missions  include  area  search,  survey  data  gathering,  and  mobile  sensor 
placement,  as  well  as,  remote  sensor  capabilities  for  both  surface  ships  and  submarines. 
Clearly,  these  unmanned  vehicles  will  be  used  to  extend  man's  presence  in  this  hostile 
environment. 

Mission  planners  use  waypoint  path  platming  to  direct  the  motion  of  AUVs  These 
waypoints  are  selected  to  avoid  known  undersea  obstacles  with  a  minimum  safety  distance 
and  are  given  a  corridor  with  a  distance  either  side  of  the  directed  path.  A  speed  of 
advance  (SOA)  is  used  so  that  the  planner  knows  were  the  vehicle  is  supposed  to  be  along 
the  path  at  all  times.  This  allows  flexibility  in  water  management  to  allow  other  vehicles 
access  as  soon  as  the  area  is  clear.  But,  if  an  unknown  obstacle  is  present  while  following 
the  planned  path  these  AUVs  must  maneuver  around  the  obstacle  while  keeping  the 
vehicle  as  close  to  the  directed  path  as  possible. 

Autonomous  vehicle  studies,  up  till  now,  have  focused  on  following  a  path  defined  by 
straight  lines.  Linear  controls  have  been  used  extensively  in  these  situations  assuming 
small  motions  about  a  nominal  straight  path.  The  simplest  guidance  scheme  used  waypoint 
aiming,  sometimes  referred  to  as  line  of  sight  steering,  (as  in  "proportional  navigation") 
with  a  separate  constant  speed  controller.  A  new  heading  was  calculated  to  the  waypoint 
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based  on  current  position  estimates.  This  design  allowed  the  vehicle  to  wander  off  the 
proposed  track  and  could  produce  a  significant  heading  difference  from  the  desired  when 
approaching  the  waypoint  [Ref  1].  Due  to  the  added  distance  required  to  travel  in  these 
cases  synchronous  time  of  arrival  is  difficult.  This  design  is  improved  by  adding  a  speed 
controller  to  change  speed  as  necessary  to  reach  the  waypoint  on  time 

At  the  next  level  of  complexity,  cross  track  error  can  be  used  to  control  heading  to 
regain  track  with  a  separate  speed  controller.  But  again,  these  systems  inherently  maintain 
a  straight  track  which  may  be  sufficient  in  open  ocean,  but  could  cause  problems  in  the 
performance  of  obstacle  avoidance  maneuvers  in  confined  waters  There  may  be  certain 
times  when  a  vehicle  needs  the  ability  to  follow  a  nonlinear  path,  as  when  required  to 
reflexively  avoid  obstacles  None  of  the  present  control  schemes  proposed  have  that 
ability. 

When  tracking  a  straight  line  the  path  velocity  is  constant  thus  the  state  variables'  rate 
and  acceleration  don't  change.  To  follow  a  nonlinear  path  requires  a  nonlinear  control 
design  and  a  more  robust  control  system  capable  of  handling  the  additional  dynamics 
involved.  Nonlinear  control  theory  has  only  recently  been  developed  and  is  not  yet  well 
established  for  a  Multiple  Input-Multiple  Output  (MIMO)  control  system  [Ref  2] 

The  purpose  of  this  thesis  is  therefore  two  fold;  firstly,  to  raise  the  precision  in 
vehicle  motion  control  capability  and  explore  the  use  of  nonlinear  tracking  control  theory 
for  submarine  path  tracking;  and  secondly,  to  generate  continuous  curvature  paths  as 
desired  paths  that  would  balance  the  tracking  of  the  nominal  path  while  avoiding  detected 
obstacles. 
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B.  OVERVIEW  OF  NAVAL  POSTGRADUATE  SCHOOL  AUV  PROGRAM 


The  AUV  program  at  the  Naval  Postgraduate  School  (NPS)  began  in  1987  with  the 
sponsorship  of  the  Naval  Surface  Warfare  Center  The  project  involves  joint  effort  from 
the  Mechanical  Engineering,  Computer  Science,  and  Electrical  and  Computer  Engineering 
Departments  and  is  focused  on  the  control,  at  both  the  Mission  and  Execution  levels  of  an 
experimental  vehicle  -  the  NPS  AUV  II 

The  NPS  AUV  II  is  a  seven  foot  autonomous  vehicle  It  was  designed  using  the 
Total  Ships  System  Engineering  approach.  This  integrated  approach  involved  an  iterative 
design  spiral  weighing  tradeoffs  in  the  following  systems  Hull,  Energy  Storage  and  Power 
Plant,  Vehicle  Motion  Control,  Sensor  Suite,  Navigation  and  Guidance,  and  Machinery 
Monitoring,  [Ref  3]  Figure  I-l  shows  the  configuration  of  the  AUV  II  and  its  internal 
equipment  arrangement  The  AUV  II  contains  its  own  internal  power  supply  using 
rechargeable  batteries  and  its  own  guidance,  control  and  navigation  computers  for 
autonomous  operation  It  has  a  sonar  suite  that  consists  of  a  200  KHz  fathometer,  a 
750  KHz  high  resolution  sector  scanning  sonar  and  a  1  MHz  pencil  beam  profiling  sonar 
No  external  tether  is  required 

The  AUV  II  guidance  system  consists  of  an  autopilot  and  associated  guidance  laws 
The  autopilot  is  responsible  for  stable  control  of  the  vehicle  in  achieving  desired  course, 
speed  and  depth.  The  guidance  law  converts  the  vehicle  position  and  orientation  into  a 
global  reference,  combines  this  with  the  desired  position  and  orientation  thus  generating  a 
global  error  signal  for  the  autopilot.  Vehicle  orientation  control  is  accomplished  with  the 
use  of  bow  and  stem  rudders  and  planes  as  well  as  four  cross  body  thrusters.  Longitudinal 
velocity  is  controlled  through  shaft  RPM,  and  depth  control  through  bow  and  stem  planes. 
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Figure  I-l  AUV  II,  Internal  Equipment  Arrangement 
C.  SCOPE  OF  THIS  THESIS 

This  thesis  addresses  only  a  small  area  of  the  autonomous  control  problem  for  the 
NPS  AUV  II  dealing  with  horizontal  plane  nonlinear  control  using  Multiple  Input-Multiple 
Output  (MIMO)  Sliding  Mode  Control  to  follow  continuously  changing  curved  tracks. 
First  we  develop  the  nonlinear  controller  for  the  AUV  and  demonstrate  its  effectiveness  at 
following  nonlinear  paths.  We  then  develop  a  path  generation  routine  which  changes 
curvature  of  the  path  based  on  sensor  measurements  and  orientation  parameters.  This  is 
then  applied,  in  simulation,  to  reflexive  obstacle  avoidance  in  a  mine  search  and 
localization  mission  From  this  routine  the  vehicle's  controller  is  provided  a  path  for  input. 

This  path  planning  scheme  is  not  based  on  an  optimization  which  would  require 
extensive  computational  time.  This  scheme  will  react  to  its  environment.  An  algorithm  that 
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is  totally  reflexive  based  on  current  information  would  provide  a  control  signal  within  the 
step  time  of  the  execution  level  controller  (0.1  sec),  [Ref  4],  In  j^ra-  ^ice,  this  system  will 
employ  sonar  detection  of  obstacles  within  its  field  of  view  and  use  range  and  bearing  to 
calculate  the  avoidance  command.  This  will  be  added  to  the  steering  order  to  regain  the 
nominal  path  and  generate  a  steering  function  to  create  the  avoidance  path  for  the  AUV  to 
follow. 

D.  THESIS  OUTLINE 

Chapter  II  will  discuss  the  theoretical  background  of  sliding  mode  control.  It  will 
include  the  conditions  of  existence  and  stability  of  the  sliding  surface,  sliding  surface 
design,  and  sliding  mode  control. 

Chapter  III  will  present  the  AUV  equations  of  motion  and  develop  the  control  law 
for  the  nonlinear  model.  Sliding  mode  parameter  selection  is  critical  to  the  operation  of  the 
sliding  surface.  Parameter  selection  will  be  discussed  and  several  simulation  runs  following 
curved  paths  will  be  demonstrated 

Chapter  IV  will  present  the  theoretical  background  for  continuous  curvature  track 
generation.  This  will  include  kinematics,  path  keeping,  and  obstacle  avoidance  algorithm 
generation. 

Chapter  V  demonstrates  the  effectiveness  of  the  AUV  to  follow  a  track  generated 
from  the  obstacle  avoidance  routine  using  the  control  system  designed  Discussions  of  its 
limitations  will  be  included. 

Chapter  VI  presents  the  conclusions  of  this  research  and  provides  recommendations 
for  future  work. 


5 


1 


n.  THEORETICAL  BACKGROUND  OF  SEffilNG  MODE  CONTROL 

A.  GENERAL 

In  a  system,  modeling  imprecision  comes  from  uncertainty  about  the  system  and  the 
simplified  representation  of  the  dynamics  involved  Modeling  inaccuracies  can  have  strong 
adverse  effects  on  the  performance  of  control  systems.  There  are  two  approaches  to  deal 
with  this  problem;  use  adaptive  control,  which  modifies  or  updates  the  parameters  within 
the  controller,  or  use  a  robust  control  that  is  relatively  unaffected  by  system  parameter 
inaccuracies. 

One  approach  to  robust  control  is  through  the  use  of  a  sliding  mode  methodology. 
The  premise  behind  this  method  is  that  it  is  easier  to  control  a  I®'  order  nonlinear  or 
uncertain  system  than  it  is  to  control  a  general  system  described  by  an  n“’  order  differential 
equations.  In  effect  we  replace  an  n*  order  system  with  an  equivalent  P'  order  problem.  It 
is  then  possible  to  show  that  "perfect"  performance  is  attainable  in  the  equivalent  system 
even  in  the  presence  of  arbitrary  parameter  inaccuracies.  But,  there  is  a  price.  Extremely 
high  control  activity  is  required  which  also  can  add  to  the  modeling  uncertainty.  Thus  a 
tradeoff  can  and  should  be  evaluated  balancing  reduced  control  activity  at  the  cost  of 
acceptable  tracking  performance  for  given  parameter  uncertainty.  In  this  chapter,  an 
outline  of  the  design  methodology  for  control  of  a  general  nonlinear  plant  is  given  as  an 
introduction  to  Sliding  Mode  Control. 


B.  SLIDING  SURFACE  DESIGN 

The  objective  is  to  find  a  sliding  mode  control  law  for  an  n*  order  system  of  the  form 


where  the  scalar  x  is  the  output  of  interest  ,the  scalar  u  is  the  control  input,  and 

x~[x  X  .  .  (II-2) 

is  the  state  vector.  The  functions  /(x)  and  6(x)  are  not  exactly  known,  but  the 
inaccuracies  are  bounded.  The  problem  then  is  to  get  the  output  x  to  track  a  specific  time 
varying  command,  x^,  in  the  presence  of  model  uncertainty.  We  assume  that  the  system  is 
initially  in  equilibrium  wher^: 

x(0)  =  x,(0).  (II-3) 

Let  x  =  x-Xj  be  the  tracking  error  of  the  single  variable  x.  If  we  define  a  sliding 
surface  by 

a(x,r)  =  (A^  +  ir'i  =  0  (IM) 

and  X  is  defined  as  a  positive  constant,  then  if  n=2  the  sliding  surface  becomes 

ct=Ax  +  x.  (II-5) 

o  is  then  the  weighted  sum  of  the  position  and  velocity  errors.  Now  the  problem  of 
tracking  x=x^  is  equivalent  to  remaining  on  the  surface  (a=0)  for  t>0  given  the  initial 
conditions.  Therefore  o=0  represents  a  linear  differential  equation  with  a  unique  solution 
X  =  0  and  an  eigenvalue  1/X.  The  scalar  o  can  be  kept  at  zero  using  the  control  u  such  that 
when  cr;^  0, 

where  7  is  a  positive  constant.  This  states  that  the  rate  of  change  of  the  squared  "distance” 
to  the  surface  decreases  along  all  paths.  This  constrains  the  trajectory  to  point  to  the 
surface  and  once  on  the  surface  to  remain  on  the  surface.  Thus  even  with  disturbances  and 
parameter  inaccuracies  the  system  maintains  a  on  the  surface  and  tracking  errors  tend 
exponentially  to  zero  with  a  time  constant  of  (n-l)X.[Ref  5] 
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In  summary,  the  idea  behind  Equations  (II-4)  and  (II-6)  is  to  follow  a  well  behaved 
function  of  the  tracking  error  and  select  the  feedback  control  law  u  in  Equation  (II- 1)  such 
that  remains  a  Lyapunov-like  function  of  the  closed  loop  system  despite  disturbances 
and  model  imprecisions. 

C  SLIDING  MODE  CONTROL 

To  illustrate  this  process  consider  the  example  of  a  scalar  system  of  single  control 
input  u 

x  =  /(x)  +  u  (11-7) 

where  x  is  the  scalar  output,  and  /is  the  nonlinear  time  varying  system  dynamics  which  are 
not  exactly  known  but  are  estimated  as  / .  The  estimation  error  in  /  is  assumed  to  be 


bounded  by  a  function  F  where 

F^/-f  (11-8) 

For  the  system  to  track  x(/)  =  Xj(t),  we  use  Equation  (II-4)  to  define  the  sliding  surface 

a=Ax  +  x.  (II-9) 

We  then  have: 

CT=Ai  +  x  (II-IO) 

and  differentiating  with  respect  to  time, 

(T=  Ax-Mj +x  =  A(/  +  u-Xj)  +  x.  (II-ll) 

As  b  approaches  zero  the  estimate  of  the  control  law  then  becomes; 

u  =  i-/+x,)-A-'i.  (11-12) 


In  order  to  satisfy  the  sliding  condition  of  Equation  (II-6)  in  the  presence  of  the 
uncertainty  in /  a  discontinuous  term  must  be  added  to  u  to  drive  it  back  to  the  surface. 
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The  hyperbolic  tangent  function  will  be  used  as  the  switching  term. 


If  =  u-ittanh(a)  (11-13) 


-3-2-10  1  2  3 

a 

Figure  II- 1 .  Hyperbolic  tangent  function  characteristics. 


It  can  be  seen  that  this  function  provides  a  smooth  transition  between  states  and  that  as 
the  magnitude  of  a  gets  larger  the  contribution  to  u  increases.  In  the  case  where  a  is  not 
zero,  tanh(a)  switches  smoothly  to  ±1  resulting  in 


^^(a^)  =  m/ -/)-ktanhi&)](7 


when  k  ^ 


tanh(aj 


is  selected. 


(11-14) 


(a^)  =  A[F-ittanh(a)]<T^-7|o|  (11-15) 

2  dt 

This  expression  is  always  decreasing  provided  |a|>  and  given  that  |F|  is  the  upper 
bound  on  F.  Choosing  k  to  be  large  enough,  ensures  robust  bounded  stability  of  the 
system.  Here  we  see  the  main  advantage  in  transforming  the  original  tracking  problem  into 
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a  simple  first  order  system.  The  simple  control  strategy  of  pushing  hard  enough  in  the 
opposite  direction  of  the  error  works  for  a  first  order  system  but,  does  not  necessarily 
work  in  higher  order  systems. 

In  order  to  design  the  control  law  for  a  single  input-single  output  system  the 
measurable  variable  y  must  be  identified  and  related  to  the  state  variables  through  a 
constant  parameter  H. 

y  =  Hx  (11-16) 

The  error  function,  a,  is  expressed  as  a  function  of  the  measurable  and  desired  parameters, 

(y=y-yj=Hx-yj  =  Hx,  (11-17) 

with  the  system  dynamics  of  the  form 

x  =  f(x)  +  bu,  (11-18) 

where  6  is  a  constant  scalar.  H  may  appear  to  be  a  nuisance  in  this  simple  problem  but  is 
necessary  when  dealing  with  MIMO  systems  working  in  real  space.  Taking  the  derivative 
of  the  error  function  and  setting  it  equal  to  the  sliding  surface  control  function  the 
following  form  is  achieved 

cr=  -^bu)- yj  = -T]ta.ni\(o)  (11-19) 

resulting  in  the  control  input 

u  =  (Hb)-'  [(j/^  -Hf)-  7tanh(cr)].  (11-20) 

Here  we  see  that  the  control  input  is  a  function  of  the  difference  between  the  desired 
velocity  and  the  product  Hf,  as  well  as  the  proximity  to  the  sliding  surface. 

In  more  complex  multivariable  systems  it  may  not  be  this  easy  to  isolate  the  control 
input.  The  ability  to  solve  for  u  may  require  a  higher  order  a  to  be  used  until  the  control 
input  can  be  identified. 

CT=  x  + A,x  + A2X+...  (11-21) 
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The  use  of  Equation  (11-20)  with  the  system  in  Equation  (11-18)  then  leads  to  the  dynamic 
response  of  a  given  by  Equation  (11-19)  but  driven  by  uncertainties  in  /,  and  leads  to  both 
o(/)  and  therefore  x  tending  to  a  bounded  constant  as  /  ->  oo. 
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in.  SLIDING  MODE  CONTROL  FOR  AUVS  IN  THE  HORIZONTAL  PLANE 


A.  EQUATIONS  OF  MOTION 

The  design  of  the  NPS  AUV  n  tracking  control  system  begins  with  identifying  the 
equations  of  motion  of  a  physics  based  model.  The  six  generalized  equations  of  motion  for 
a  submerged  vehicle  are  found  in  Principles  of  Naval  Architecture  [Ref  6].  In  developing 
the  model  for  the  AUV  II  several  assumptions  were  made  to  simplify  and  limit  ourselves 
to  the  horizontal  plane.  The  assumptions  are  summarized  below; 

1 .  The  AUV  II  is  neutrally  buoyant;  W=B. 

2.  The  AUV  II  is  symmetrically  loaded  in  the  transverse  direction  (yG=0  and 
yB=0),  and  the  vertical  center  of  buoyancy  is  midway  between  the  top  and 
bottom  of  the  AUV  II  (zb=0). 

3.  The  counter-rotating  propellers  produce  no  yaw  moment  (Nprop=0). 

4.  The  products  of  inertia  about  the  body  system  are  zero  because  the  AUV  II 
possesses  two  axes  of  symmetry. 

5.  The  AUV  II  acceleration  rates  are  small  enough  that  propeller  slip  can  be 
neglected. 

6.  The  effect  of  cross-coupled  hydrodynamic  added  mass  coefficients  can  be 
neglected  in  most  cases,  again  because  of  the  AUV  II  geometric  symmetry. 

The  resulting  generalized  equations  of  motion  for  horizontal  plane  motion  including 
surge,  sway,  and  yaw  are  presented  below,  where  n  and  DR  are  generalized  controls. 
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(m-  )U  =  {mVR  -  X^^U\U\  + ) 

(m-Y^)v  =  (-mRU  +YyVU  Y^RU -i- (Y^,  +Y^)U^DR)  (III-l) 

Uzz  -  +  N,RU  )U^DR) 


TABLE  III-l  AUV  B  HYDRODYNAMIC  COEFFICIENTS 


Coefficient 

Value 

Coefficient 

Value 

Uft) _ 

7.3 

1.94 

RBRIHI 

laamm 

X.,., 

« _ 

32.2 

Xprop 

45 

X. 

-0. 00282- '/2pL3 

Yv 

-0.03896'/2pL2 

N. 

-0.00769-'/2pL3 

Yv 

-0.03430-'/2pL3 

Nv 

-0.001 78- '^pL-* 

Y. 

0.01187-'/2pL3 

N. 

-0.01022-'/2pL-‘ 

Y. 

-0.001 78- '/2pL4 

N, 

-0.00047- '/2pL5 

Y„.. 

0.02345-'/2pL2 

N„.. 

Y 

0.02345-'/2pL2 

The  X,  Y,  and  N  terms  are  the  hydrodynamic  influence  coefficients  as  presented  by 
Warner  [Ref  7],  They  relate  the  force  or  moment  on  the  vehicle  for  a  given  change  in  the 
vehicle  motion  of  interest  and  are  often  converted  from  nondimensional  quantities 
involving  vehicle  length  and  forward  speed.  For  example,  the  change  in  force  in  the  Y 


direction  for  a  given  change  in  sway  velocity  or  a  given  stem  rudder  angle  is  annotated  by, 

y  .  \  ^ 


1  ^ 

= - and 


U  dy 


Y  = 

*t)fU 


cDRs 


To  relate  vehicle  motion  to  a  global  coordinate  system  a  transformation  is  required 
that  relates  vehicle  surge  and  sway  velocities  to  the  global  velocities  and  yaw  rate  to  the 
global  Euler  angle  rate. 
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^  =  7? 

x  =  L^cos('F)-Fsin('{') 
>'  =  t/sin('F)  +  Fcos('F) 


(111-2) 


Most  control  systems  deal  with  Equations  (m.  1)  and  (III.2)  as  separate  guidance  and 
autopilot  systems.  Though  auotpilots  in  the  past  have  incrementally  segmented  the  path  to 
effectively  form  a  curved  path,  they  lack  of  rate  and  acceleration  terms,  for  feed  forward, 
which  more  accurately  define  the  curved  path.  But,  when  trying  to  follow  a  global  track 
with  sliding  modes  these  equations  are  integral  to  each  other.  By  coupling  them  together 
into  a  single  function  the  total  effect  of  their  relationships  form  a  single  control  system 
This  provides  us  with 

(m-jrJ  0  0  0  0  op 

0  0  0  0  0  F 

0  0  (l22-N^)  0  0  0  R 

0  0  0  1  0  0  'F 

0  0  0  0  1  0  i 

0  0  0  0  0  1  jj- 


mVR-X^U\U\  1  0 

-mRU  +  YyVU  +  Y,RU  0  {Y^,  +  Y^  )U^ 

NyVU  +  ^  0  ( )C/^  T  ' 

R  ^0  0  [dR^ 

C/cos(T)-Fsin('F)  0  0 

(/sin('F)  +  Fcos(4')  0  0 

(III-3) 

To  reduce  the  complexity  of  this  equation  for  subsequent  calculations  M,  F(X)  ,  G  and  5 
will  replace  their  respective  matrices  to  form  the  following  equation 

MX  =  F{X)-^GS  (IIM) 

X  =  A/-'  {F(Ar)  +  G5}  =  M-'F{X)  +  M'GS.  (III-5) 
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B.  CONTROL  INPUT  DEVELOPMENT 


To  develop  a  control  system  to  follow  a  path  defined  by  global  x  and  y  coordinates 
there  must  be  at  least  two  control  inputs.  These  two  inputs  will  be  RPM  squared  (/P)  for 
longitudinal  speed  control  and  rudder  order  {DR)  with  both  bow  and  stem  rudders 
operating  together  but  in  opposite  directions.  If  is  defined  as  the  desired  positions  in  the 
global  system  then: 

n =[;;].  (...-6) 

If  Y  is  defined  as  the  current  position  then  the  system  states  {X)  can  be  related  to  Y  by  H: 

y  =  M  ^[000010 
[0  0  0  0  0  1 

To  control  Y  to  Yj  an  error  function  is  required.  Y  defines  the  error  in  terms  of  global 
position. 

Y  =  Y-Y,=HX-Y,  (III-8) 

In  designing  the  sliding  surface  for  this  system,  o  will  be  initially  set  equal  to  the  error 
function  where  it  will  be  shown  to  be  insufficiently  rich  in  dynamics  as  required  to  extract 
the  solution  for  5 


a=?=HX-Y,.  (III-9) 

Taking  the  derivative  of  o  and  setting  it  equal  to  the  normalized  sliding  surface  control 
function  the  following  equations  are  obtained. 


<7  =  f  = -  r/tanhf-l 

\<P) 

(  HX  —  Y 

a=  HM-'F(X)  +  HM-'GS-  Y,=-  ^tanh  - ^ 

V  <P 


(III- 10) 
(III-ll) 
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Here  we  see  that  with  two  states  being  tracked  t]  = 


>1  0 

0  TJ. 


tanh(x-x^) 

<P^ 

9>2 


.  Solving  for  S 


■=(/ft/-'G)  '  Y,- 


HM-'F(X)-rii2aAi 


HX-Y. 


(Ill- 12) 


5  is  shown  to  be  a  function  of  the  difference  between  the  path  and  vehicle  velocities  and 
sliding  surface  control  function.  But 


(III- 13) 


and  singularly  not  invertable  thus  a  control  can  not  be  found  and  the  degree  of  o  must  be 
increased. 

Because  no  solution  exists  in  this  case,  a  higher  order  sliding  surface  must  be  used 


and  we  redefine  o-  as  a  function  of  position  and  velocity  errors 


where  X 


a=Y  ^  X?  =  {HX  -Y,)^  x(hX  -Y^) 
PA,  0 1 

i  =  and  are  time  constants 

0  Xj 


(III- 14) 


and  are  time  constants  associated  with  a  desired  rate  of  decay  of 


positional  error.  Taking  the  derivative  of  a  and  setting  it  equal  to  the  sliding  surface 
control  function  we  obtain 

b=?  +  Xf  =  (HX-Y,)  +  x{HX-Y,) 

=  (h(m-'F{X)  +  M-'GS)  -  +  x{h(m-'F(X)  +  M'^G's)  -  Y, )  (III-15) 

( 

=  -  r/tanh  — 

\<P) 

The  F{X)  term  is  the  derivation  of  F(X)  with  respect  to  time.  Within  the  matrix  format 


this  must  be  done  using 

F(Ar)  =  — =  — —  =  F^A’  ;  F.  eSR*"®,  A' 

^  a  cX  a 


(III- 16) 
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where 


mR 

mV 

0 

0 

0 

-mR  +  YyV  +  Y^R 

YyV 

{Y^-m)U 

0 

0 

0 

NyV  +  N^R 

NyU 

N,U 

0 

0 

0 

0 

0 

1 

0 

0 

0 

008(4^) 

-sin('F) 

0 

-(/sin(4')-rcos(T) 

0 

0 

sin('P) 

008(4^) 

0 

(/cos(4')-Fsin(4') 

0 

0 

(III  17) 


The  derivative  of  a  equation  then  becomes: 

(h(m-'f{X)  +  M'Gs)  -  yj + m'gs)  -  Y, ) 

=  -  r/tanhf — 1 


(III- 18) 


Inserting  the  equation  for  X,  realizing  that 
obtain 


7 

/  ,  \  fo  o1 

(//Ay-'G)  =  [o  oj 


and  solving  for  5  we 


s=[xhm-'FxM-'g)' 


{y^-HM-'F{X))^k[Y,  -HM-'FxM-'F{X))  -  ^tanhf- 


(III- 19) 


The  control  variable  is  now  a  function  of  the  differences  in  the  path  and  vehicle's 
acceleration  and  velocity  terms.  The  transformational  matrix  term  for  6  becomes 


m-Y, 


m-Y, 


■cosC'F)  -(/' 
-sin('F)  U' 


2  f  ^DKi  ^DRb 


1 

2  ^DRj  ^DRb 

m-Y,, 


sin('y) 

cos('F) 


(III-20) 


This  matrix  is  square  and  therefore  invertable  as  long  as  the  determinant  is  not  zero. 
While  Equation  III-20  would  normally  produce  the  required  control  law,  one  particular 
condition  still  persists  where  the  control  is  not  recoverable,  i.e.  Yjyj^^  =  -Y^fd,. 
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Physically  this  means  that  under  this  condition,  vehicle  sway  motions  would  not  be 
directly  influenced  through  the  rudder  command  and  it  follows  that  path  tracking  to 
position  commands  is  not  possible  this  way  To  resolve  this  dilemma  another  independent 
control  variable  is  introduced  The  AUV  II  has  the  capability  to  order  its  rudders 
independently  but  has  not  incorporated  this  in  the  past.  To  do  this,  the  bow  and  stem 
rudders  wilt  be  decoupled  and  controlled  independently.  There  must  be  at  least  as  many 
independent  control  variables  as  there  are  outputs  to  control,  otherwise  the  transformation 
matrix  will  not  have  sufficient  rank  and  5  could  not  be  found  directly.  Heading  will 
correspondingly  be  added  to  the  output  variables  to  retain  squareness  in  the  system  input- 

output  behavior  The  output  vector,  Y,  must  be  redefined  as 
’Tl  [0  0  0  1  0  O' 

y=  X  =ooooiojir  (111-21) 

y  0  0  0  0  0  1 


and  the  desired  path  coordinates  must  be  redefined  as 


(III-22) 


The  control  function  G  and  control  variable  6  must  be  redefined 


(III-23) 


Following  the  same  procedure  as  previously  and  solving  for  the  control  input 
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<5=(aW/vA/-'g)  ‘  X 

(Y,-HM-'F{X))^x(y,-HM-'FxK1''F{X))-  ;7tanhf-ll 

I  \<P)} 


(III-24) 


Simplifying  the  equation. 


(  ( nW 

S=(WM-'FxM-'G)'  /  +  A/-I7tanh  -  . 

I  \^)) 


(III-25) 


where  /  and  /  are  the  feed  forward  terms  from  rate  and  acceleration  errors.  The 
transformation  matrix  becomes 


ANM-'FM''G  = 


AjCos(t)^, 


m-Y., 


>l3sin('F)A', 


m-Y^ 


1 

I.-N^ 

-X,U^  sin(4<)y™. 

L-N, 

-X,U^  sinC'F))',*. 

m-Y, 

m-Y, 

X,U’  coi'¥)Y^, 

A,(/=  cosC't'))',*. 

m-Y, 

m-Y, 

(111-26) 

This  matrix  is  invertable  with  the  following  analytical  solution. 


cosC'fX/h-^j^) 


sin(T)(m-^^.) 

^i^PROP 


^2^  PROP  ^i^PROP 

-^oRsilu  -  )  -sin('P)A^^  (m-Y,)  cos(^)N^^(m  -  Y, ) 


A|C/  ^DRb^DRj^  ^2^  (^ms^DRb  ^  DRb^DRj^  DRs^Cmb  ^  DRb^I^^ 

YoRbi^u-^A)  sin{'¥)N^,{m-Y,)  -cos{'¥)N^,{m- Y,) 


A|l/  iN[jnjYpffi,  f^DRb^DRj)  ^2^  ^^DRs^DRb  ^ DRb^DRj)  ^3^  DRs^Mb  ^ DRb^DRs) 


(III-27) 


To  simplify  for  future  calculations,  the  constants  are  combined  and  reduced  to; 


cos('F)A/  sinWM 


-K 


^2 

-sin('F)A^. 


Y,  sin('F)Ar, 


A.f/* 


cos('F)A^. 

-cohf)N, 

X^U^ 


(III-28) 


This  matrix  is  clearly  singular  at  U=Q  when  the  vehicle  would  be  uncontrollable.  This  work 
will  not  address  this  condition.  The  transition  to  hover  mode  will  require  extensive 
additional  research.  We  will  operate  well  above  this  region. 

Solving  for  each  individual  control  input 


=  m\ 


-  "•  ■  ’■  “*(  -  ’'““I 


(III-29) 

Notice  that  each  control  is  dependent  on  the  choice  of  sliding  surface  coefficients.  It 
follows  that  control  design  parameters  must  be  carefully  selected  and  scaled  as  appropriate 
so  that  the  multivariable  controller  becomes  balanced  without  excessive  saturation  on  any 
channel. 


C.  PARAMETER  SELECTION  PROCEDURES 

The  parameters  X  tj.  and  <p  are  selected  by  the  control  designer  based  on  operational 
characteristics  of  the  vehicle  and  track.  The  parameter  A,  is  a  term  relating  the  ratio  of 
errors  in  the  output  variables'  displacement  and  rate.  This  determines  if  the  displacement 
or  rate  terms  dominate  on  the  sliding  surface.  If  X=l,  then  displacement  and  rate  terms  are 
equally  weighted.  This  parameter  is  related  to  the  response  time  constant  for  the  particular 
output  variable.  The  parameter  q>  controls  the  sensitivity  to  saturation  of  each  individual 
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sliding  surface  function.  Too  small  a  value  will  saturate  the  sliding  surface  at  small  error 
levels  and  will  give  rise  to  high  levels  of  control  activity.  To  temper  the  selection  of  q>,  we 
must  recognize  that  as  <p  grows  so  do  the  tracking  errors.  Thus  the  control  designer  must 
select  a  prudent  value.  The  parameter  t}  is  for  scaling  the  control  input.  If  is  not  large 
enough,  the  system  can  become  unstable  when  parameter  uncertainty  exists  Too  large  a 
value  will  cause  chatter  and  too  small  will  allow  errors  to  propagate.  The  best  value  would 
be  just  at  the  point  of  chatter  but  the  operator  must  use  trial  and  error  to  find  this  value 
and  in  practice,  sensor  noise  vnll  be  a  reason  to  use  as  small  a  value  as  possible  consistent 
>vith  robust  stability. 

Through  much  trial  and  error  the  parameters  used  for  ail  simulations  were; 


<P  = 


1 

0 

0 


0 

10 

0 


0 

0 

10 


TJ  = 


1 

0 

0 


0 

10 

0 


0 

0 

10 


(111-30) 


(III-31) 


and 


0.075  0  0 
0  5  0 

0  0  5 


(III-32) 


The  values  for  (p  were  selected  so  that  significant  errors  would  be  required  before 
saturation  developed.  The  values  of  7  were  selected  to  offset  the  reduction  in  the  sliding 
surface  function  caused  by  the  q>  term.  Since  most  operations  will  occur  while  C7<  0.4^ 
then 


_tanh(CT)  ^  _ 
7 - =  cr 


(III-33) 
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The  values  of  X  were  selected  to  ensure  that  heading  errors  dominated  the  rudder  controls 
In  this  mode  rudders  will  operate  in  opposite  direction  for  maximum  turning  rate  As 
positional  errors  grow  they  will  mitigate  this  effect  turning  the  stem  rudder  as  to  gain  the 
path  thus  reducing  the  turning  rate  of  the  vehicle. 

D.  AUV  SIMULATION  RESULTS 

To  validate  this  MIMO  control  theory  we  will  generate  curved  paths  using  various 
continuous  curvature  shapes.  Curved  paths  developed  from  zero,  sinusoidal  and  sawtooth 
shaped  curvature  were  generated  as  commands  to  track  using  the  control  laws  developed. 
These  examples  will  demonstrate  that  the  MIMO  control  system  does  provide  adequate 
control  in  an  AUV  application 

Figure  III-l  illustrates  a  straight  line  path  represented  by  the  dashed  line  in  the 
"POSITION"  frame.  The  simulated  AUV  path  is  the  solid  line.  The  initial  position  was  off 
the  track  starting  point  by  0. 1  feet  in  both  the  x  and  y  directions.  As  simulations  began  the 
heading  errors  were  small  thus  the  position  errors  dominate  and  the  controller  ordered  the 
bow  and  stem  mdders,  DRb  and  DRs  respectively,  to  work  together  in  the  same  direction 
causing  the  AUV  to  crab  sideways  regaining  track. 

Figure  III-2  demonstrates  the  controllers  ability  to  track  a  slowly  varying  sinusoidal 
curvature  with  a  maximum  curvature  of  0.0375(ft-i).  In  this  situation  the  heading  error 
dominates  until  heading  is  nearly  matched.  The  rudders  orders  are  proportionally  in 
opposite  directions  to  establish  the  turn  rate  required.  As  position  errors  grow  they  begin 
to  dominate  and  the  stem  rudder  slowly  shifts  direction  attempting  to  regain  track  with 
lateral  force.  Notice  that  the  bow  rudder  follows  the  curvature  shape  while  the  stem 
rudder  compensates  for  position  offset. 
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The  Figure  ni-3  simulation  increases  the  magnitude  of  curvature  to  develop  a  tight 
turn.  The  heading  error  is  large  dominating  the  rudder  commands  until  the  rudder  limits 
are  reached.  This  clipping  action  allows  the  heading  and  position  errors  to  grow  more 
rapidly.  As  the  position  errors  grow  they  contribute  more  to  the  rudder  controls  shifting 
the  direction  of  the  aft  rudder  in  an  attempt  to  use  the  sway  force  to  regain  the  path. 

In  simulation  Figure  DM,  with  a  sawtooth  shaped  curvature,  the  effects  of  the 
discontinuous  curvature  rate  were  tested.  The  discontinuities  did  cause  transients  but  had 
little  effect  on  the  performance  of  the  controller. 

In  Figure  III-5,  a  circle  of  twenty  foot  radius  was  tracked.  This  constant  curvature 
path  allowed  the  controller  to  achieve  a  constant  heading  and  positional  error.  The  x  and  y 
position  errors  are  sinusoidal.  The  sum  of  there  errors  are  a  constant  magnitude  The  bow 
and  stem  rudders  achieve  a  steady  state  position  where  the  bow  rudder  generates  the 
turning  rate  and  the  stem  rudder  generates  a  sway  force  to  hold  a  constant  path  offset. 

Figure  III-6  used  the  same  initial  conditions  of  Figure  III-2  but  a  0.2  knot  current  in 
the  X  direction  was  applied.  The  control  system  reaches  a  steady  state  offset  in  position  to 
develop  the  rudder  order  required  to  compensate  for  the  current.  Also  note  that  the  speed 
order  was  reduced  due  to  current  assisting  in  the  path  direction.  This  figure  illustrates  the 
need  for  integral  control  to  remove  the  steady  state  bias.  But  integral  control  causes 
problems  in  track  following.  The  response  from  integral  control  is  second  order  and  can  be 
very  oscillatory.  With  any  form  of  integral  control,  however,  care  must  be  taken  to  avoid 
the  reset  windup  problem.  The  current  system  with  its  associated  errors  is  "  good 
enough". 
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IV.  GENERATION  OF  DYNAMICALLY  STABLE  CURVED  PATHS 


It  has  been  shown  that  a  nonlinear  control  system  can  be  designed  to  drive  a 
submarine  vehicle  and  follow  a  continuously  changing  curved  path.  Now  we  are  interested 
L>  developing  a  path  for  the  vehicle  that  will  inherently  follow  a  directed  waypoint  track 
while  avoiding  objects  such  as  in  a  mine  field.  This  process  consisted  of  three  steps; 

1 .  Develop  a  waypoint  track  following  algorithm. 

2.  Develop  a  sensor  model  to  determine  if  a  mine  is  within  sensor  range.  Then 
generate  a  change  in  path  curvature  of  sufficient  magnitude  to  avoid  the 
obstacle  without  exceeding  the  control  capabilities  of  the  vehicle. 

3.  Incorporate  multiple  way  points  in  the  directed  path 


A.  PATH  KINEMATICS 

Consider  a  path  whose  incremental  change  is  measurable  over  a  constant  time 
interval  At.  The  incremental  translational  motion  is  As  along  the  path  for  each  data  point 
and  its  incremental  orientation  change  is  A0  in  each  incremental  time  interval.  Therefore; 

U  =  —  =  —  ->  As  =  UAi,  (IV-1) 

dt 


and 


AQ  dQ 
At  dt 


A0  =  0A/ . 


(IV-2) 


Assume  that  the  path's  configuration  is  q(s)=Xx(s),y(s),0(s))  at  s  and  the  incremental 
values  of  As  and  A0  are  given  for  the  next  time  interval.  Knowing  this  we  can  "DEAD 
RECKON"  the  path  by  Euler  integration.  The  next  orientation  can  be  determined  by; 
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x(s  +  As)  =  x(s)  +  Ascos(0(s)  + A0/2) 
y(s  +  As)  =  y(s)  +  Assin(0(s)  + A0/i). 
0(s  +  As)  =  0(s)  +  A0 


(IV-3) 


Thus  the  next  configuration  is; 


q(s  +  As)  =  (x(s  +  As),  y(s  +  As),0(s  +  As)).  (IV-4) 

By  approximating  this  curve  as  a  circular  arc  with  an  average  curvature,  the  radius  of 
curvature  then  becomes: 

r  =  -^  =  ^  ifA0^O.  (IV-5) 

A0  0 

The  path  curvature  (/O  is  defined  as  the  inverse  of  the  radius  of  curvature  and  can  be 
expressed  in  terms  of  the  yaw  rate  and  path  speed  as: 

=  if  A0^O  (IV-6) 

r  As  U 

Figure  IV- 1  illustrates  the  geometry  of  the  path.  If  we  evaluate  the  length  of  the  straight 
line  segment  (d)  over  the  time  interval  At  then; 


dll  .  rA0^ 

r  I  2  J 

Solving  for  d: 

c/  =  2rsin^^j. 


(IV-7) 


(IV-8) 


We  can  now  rewrite  the  kinematic  equations. 

x(s  +  As)  =  x(s)  +  dcos(0(s)  + A0/2) 

y(s  +  As)  =  y(s)  +  dsin(0(s)  +  A0/2).  (IV-9) 

0(s  +  As)  =  0(s)  +  A0 
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Figure  IV- 1 .  Path  Geometry 


B.  PATH  GENERATION 

Stable  paths  may  be  generated  if  path  curvature  is  controlled.  In  particular,  a  path 
may  be  generated  and  forced  to  regain  some  desired  path  defined  by 
,,y,m)  if  (‘O,AA',A0)  — >  0  as  /  — ►  oo  where  D  is  the  perpendicular 

distance  from  the  current  orientation  to  the  desired  path  and 


AK-=  (*•-*-_) 

(IV-IO) 

40  =  (0-0.) 

(IV-11) 

Then  curvature  can  be  controlled  by  a  specification  for  dA^ds,  which  we  call  the  "Steering 
Function",  to  be  a  continuous  function  of  the  path  variables.  This  would  ensure  K  is 
continuous  and  smooth. 
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(IV-12) 


This  then  reduces  the  steering  function  from  a  three  degree  of  freedom  function  to  a 
single  degree  of  freedom,  and  Ir*  becomes  a  characteristic  length.  This  single  variable  k 
adjusts  the  sensitivity  of  the  control  to  give  rapid  or  gentle  return  to  the  directed  track  and 
ensures  a  stable  result.  As  k  becomes  smaller  the  path  is  less  responsive  reaching  the 
directed  track  more  slowly.  As  k  becomes  larger  the  path  more  rapidly  acquires  the 
directed  track. 

Once  d/T/ds  is  calculated,  it  is  used  to  change  the  current  path  curvature  (/T)  to 
merge  with  the  directed  track.  Thus  using  Euler  integration  of  the  path  kinetics,  K  is: 

A-(i)  =  /r(i-l)  +  (d;r/ds)As,  (IV-17) 
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where  A5  is  the  incremental  path  length.  The  change  in  the  orientation  (A@)  associated 
with  this  change  in  curvature  can  be  calculated: 

A0  =*•(!)•  As  aV-18) 

Using  Equation  (rV-9),  the  next  path  position  can  be  calculated  with  this  orientation 
change  and  the  incremental  path  length.  Changing  these  equations  to  the  discrete  time 
domain  we  get. 

x(i  +  l)  =  x(i)  +  U(i)cos(0(i)  +  0(i)i///2)J'/ 

y(i  +  l)  =  y(i)  +  U(i  )■  sin  (0(i  )  +  ©(/)<*/ 2^/ .  (IV-19) 

0(i  + 1)  =  0(i)  + 

This  process  progresses  through  the  entire  path  length.  This  algorithm  was  tested 
with  orientations  and  tracks  in  every  quadrant  to  ensure  validity.  Since  the  nonlinear 
controller  also  needs  commanded  velocity  and  acceleration  terms  to  define  the  feed 
forward  terms,  we  get 

x(i)  =  (/(i)  •  cos(0(i)  +  QQ)di  /2) 

y(i)  =  i/(i)  •  sin(0(i)  +  Q(i)dt  /2)  (IV-20) 

0(i)  =  (/(i)/C(i) 


This  is  a  discrete  approximation  and  will  give  errors  if  rapid  or  discontinuous  changes  in  K 
take  place. 
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The  continuous  curvature  path  has  many  uses.  It  can  be  used  to  redirect  vehicle 
motion  and  response  characteristics  Avhen  off  track  to  regain  the  desired  path.  It  allows  the 
control  designer  to  direct  a  path  to  follow  during  transition  from  one  track  segment  to 
another  at  waypoints.  Finally,  the  primary  purpose  of  this  thesis,  it  allows  easy  integration 
for  reflexive  path  generation  in  obstacle  avoidance. 

C.  OBSTACLE  AVOffiANCE  PATH  GENERATION 

Now  that  we  have  developed  a  stable  path  to  follow  the  desired  path,  we  must 
address  the  problem  of  avoiding  any  obstacles  along  the  way.  The  goal  is  to  achieve 
smooth  transition  from  track  following  to  an  avoidance  mode  so  that  dK/ds  generates 
continuous  curvature.  If  a  rule  based  switch  is  used  for  transition,  discontinuities  would  be 
introduced  in  dK/ds.  This  "jerk"  must  be  minimized  to  ensure  a  continuous  curvature.  A 
smooth  transition  into  avoidance  could  be  obtained  if  dK/ds  for  avoidance  {delK^  were 
developed  from  environmental  information  and  summed  with  the  path  following  term 
{delKp)  as  discussed  previously.  This  would  provide  a  total  dK/ds  for  path  generation. 

—  =  delK  +  delK^  (IV-22) 

For  a  smooth  transition  into  avoidance  the  del  Kg  term  should  be  zero  at  the  initial 
detection  of  an  obstacle  and  grow  as  the  range  to  the  obstacle  decreases.  For  smooth 
transition  back  to  path  following  dK/ds  should  return  to  zero  while  passing  the  closest 
point  of  approach  (CPA).  This  occurs  when  the  relative  bearing  (BRG)  to  the  mine  is  ±90’ 
.  Thus  dK/ds  will  be  a  function  of  a  range  term  (delKj)  and  a  relative  bearing  term 
ifklK^.  To  provide  control  of  the  rate  of  avoidance  a  scaling  term  (K^cale)  needed.  The 
product  of  these  terms  will  give  smooth  transition  into  and  out  of  obstacle  avoidance. 

delK,  =  delK,  •  delK,  ■  (IV-23) 
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There  were  many  iterations  in  the  functions  used  for  these  terms.  The  equation  providing 
the  best  transition  characteristics  was. 


(IV-24) 


During  path  tracking,  at  a  ten  hertz  update  rate,  obstacles  detected  within  the  sensor 
range,  considered  for  avoidance.  For  vehicle  safety  a 

mir  I  num  avoidance  range  (Rfigvoid)  ^  twenty  feet.  To  keep  from  being 

violated  the  amount  of  dif/ds  would  need  to  be  grow  significantly  as  the  vehicle 
approaches  close  to  /Wiavoij  This  could  be  achieved  if  delK^  was  inversely  proportional  to 
the  range  to  the  mine  minus  Rh^^oid  Using  this  equation,  as  Rh„^„  crosses  into 

sensor  range  the  delK^  term  is  zero  but  as  it  approaches  Rhgvoid  approaches 

infinity.  This  provides  a  smooth  application  of  dK/ds  while  preventing  the  violation  of 
Rhavoid  ^  approaches  n/2  the  delKf,  term  reduces  to  zero.  Thus  at  the  closest  point 
of  approach  (CPA)  the  value  of  dK/ds  for  avoidance  is  zero.  At  the  same  time,  Rh„i„  is 
increasing  and  the  control  mode  shifts  back  to  path  following. 

Several  simulations,  illustrated  in  Figures  IV-2  thru  IV-5,  were  run  demonstrating  the 
ability  to  develop  a  path  through  the  mine  field  without  violating  the  avoidance  range. 
These  paths  have  curvatures  well  within  the  capabilities  of  the  AUV  II  to  negotiate.  The 
maximum  curvature  required  in  these  cases  was  about  0.07(ft->).  If  the  avoidance  range 
were  reduced  the  maximum  curvature  would  increase.  If  reduced  too  far  it  would  be 
beyond  the  capability  of  the  vehicle  to  follow.  A  closer  avoidance  range  is  impractical  for 
a  mine  avoidance  role. 


As  the  mine  field  density  increases  so  does  the  value  of  dK/ds  required  to 
successfully  traverse  the  mine  field.  These  rapid  changes  in  curvature  are  due  to  shifting  in 


and  out  of  the  avoidance  criteria  in  close  proximity  to  CPA  while  close  to  the  avoidance 
range. 

D.  MINE  LOCALIZATION 

Showing  that  a  path  can  be  successfully  generated  in  real  time  through  a  mine  field 
has  many  uses  besides  the  obvious  passage  through.  AUVs  are  being  considered  for  mine 
localization  in  a  Mine  Warfare  scenario.  By  combining  a  multisegment  tracking  ability  and 
a  search  plan  along  with  this  avoidance  routine,  the  AUV  can  be  used.  A  multisegment 
path  transition  was  accomplished  by  using  a  "waypoint  reached"  criterion.  This  criterion 
checks  the  distance  to  the  next  way  point  in  each  iteration.  It  determines  a  new  speed  to 
reach  the  waypoint  on  time  and  if  the  distance  meets  the  "waypoint  reached"  criterion,  the 
next  waypoint  path  parameters  are  calculated  and  used  for  calculating  the  steering 
function  to  merge  with  the  new  desired  path.  Combining  this  with  the  avoidance  routine, 
paths  were  successfully  generated  through  several  mine  field  configurations.  Figures  IV-6 
and  IV-7  are  illustrations  of  localization  and  avoidance  behaviors  of  the  path  generation 
algorithm  for  uniformly  distributed  random  mine  fields  of  twenty  mines. 

E.  RESULTS 

Using  the  obstacle  avoidance  path  provides  excellent  control  of  the  vehicle  in  even  a 
dense  mine  field  when  all  detected  mines  are  used  to  form  the  avoidance  calculation.  This 
tool  together  with  the  mission  planners  directed  track  provides  a  safe  path  through  a 
hostile  environment.  But  as  has  been  seen  the  change  in  curvature  and  the  rate  of  its 
change  is  significant  in  the  denser  mine  fields.  There  is  a  possibility  that  as  the  avoidance 
range  for  adjacent  mines  overlap,  the  path  may  attempt  to  bisect  the  mines.  The  higher 
density  fields  are  unrealistic  in  the  mine  localization  role  but  were  used  more  to  illustrate 
the  abilities  of  the  algorithm.  The  actual  vehicle  is  not  capable  of  dealing  with  the  rapid 
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V. 


AUV  SIMULATION  USING  PATH  GENERATION 


We  have  shown  that  curvature  path  control  can  successfully  negotiate  a  mine  field 
and  that  the  AUV  MIMO  control  system  can  follow  continuous  curved  paths  But  we 
must  realize  that  we  must  not  create  a  curved  path  beyond  the  capability  of  the  AUV  to 
negotiate.  To  determine  the  limits  of  the  AUVs  maneuverability  a  simulation  was  run  with 
the  bow  and  stem  rudders  at  their  limits,  25  degrees.  They  were  positioned  in  opposite 
directions  to  maximize  the  turning  rate.  Figure  V-1  shows  that  the  maximum  curvature  in 
this  maneuver  is  about  0.16  (ft"*)  which  corresponds  to  a  tactical  diameter  of  about  12.5 
feet.  The  critical  parameter  for  maneuverability  is  the  rate  of  curvature  change  dK/ds 
must  not  change  too  rapidly  or  the  AUV  will  not  be  able  to  develop  the  turning  rate 
required  to  maintain  the  path.  If  the  turning  rate  is  too  large  heading  and  position  errors 
will  develop.  If  allowed  to  grow  unchecked  these  could  cause  instability  of  the  system. 
This  limiting  of  dK/ds  and  curvature  terms  may  prevent  successful  passage  through  the 
higher  density  mine  fields  where  higher  values  are  required. 

To  evaluate  the  combined  control  system  of  the  AUV  several  simulations  were  run. 
The  first  simulation  positioned  a  single  mine  on  the  desired  path.  Figure  V-2  shows  the 
curvature  required  to  maneuver  around  the  obstacle.  This  generated  the  avoidance  path 
depicted  in  the  POSITION  plot  as  the  dashed  line.  The  simulated  AUV  path  is  depicted  as 
the  solid  line.  The  mine  is  shown  as  the  star  with  the  avoidance  range  dotted  around  the 
mine.  The  control  system  lags  the  avoidance  path  slightly  but  position  errors  do  not 
exceed  half  a  boat  length.  This  "loose"  control  will  allow  more  uncertainty  errors  without 
adversely  affecting  the  performance. 
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The  next  simulation.  Figure  V-3,  increased  the  mine  field  density  Four  mines  are 
arranged  around  the  path.  In  this  case,  the  avoidance  command  generator  only  sees  one  or 
two  mines  at  a  time.  There  is  plenty  of  room  to  maneuver  the  AUV  between  the  mines 
before  the  next  mine's  avoidance  command  builds  to  a  point  that  it  dominates  the  control 
order. 

Increasing  the  mine  density  again.  Nine  mines  are  positioned  equally  spaced  about  the 
path.  In  Figure  V-4  we  see  that  the  increased  rate  of  curvature  does  generate  rapid  rudder 
orders.  The  rate  of  rudder  change  is  acceptable. 

In  the  last  case,  ten  randomly  distributed  mines,  were  uniformly  distributed  around 
the  desired  path.  In  a  random  distribution,  the  average  distance  between  mines  is  increased 
over  the  equally  spaced  mine  distribution  This  provides  windows  of  opportunity  for  the 
AUV  to  pass  through  which  it  would  not  have  with  an  equally  spaced  mine  field  of  the 
same  density. 

Three  general  observations  were  made  in  these  simulations.  First,  rudder  clipping  at 
25  degrees  does  not  allow  adequate  turning  rates  required  by  the  controller.  This  allows 
positional  errors  to  grow  causing  the  stem  rudder  to  shift  in  an  attempt  to  force  its  way 
back  to  track.  In  doing  this,  positional  errors  grow  even  larger  If  the  path  had  not  reduced 
its  curvature  at  this  point  allowing  the  vehicle  to  regain  track,  both  rudders  would  have 
saturated  and  control  would  have  been  lost.  This  oscillatory  path  is  very  forgiving 
allowing  vehicles  to  regain  track  once  errors  grow.  Secondl>,  the  bow  rudder  order  angle 
is  directly  proportional  to  path  curvature.  This  may  allow  the  use  of  curvature  to 
command  the  rudders  together  in  an  avoidance  and  path  following  mode.  And  thirdly,  the 
positional  errors,  while  turning  rapidly  around  an  object,  fall  outside  the  path  around  the 
mine.  Thus  the  path  errors  are  safely  conservative. 
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Figure  V-3.  AUV  Simulation:  Obstacle  Avoidance  Four  Equally  Spaced  Mines 


Curvature  and  dK/dS 
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Figure  V-5.  AUV  Simulation;  Obstacle  Avoidance  Ten  Randomly  Placed  Mines 


VL  CONCLUSIONS  AND  RECOMMENDATIONS 


A.  CONCLUSIONS 

Nonlinear  MIMO  sliding  mode  control  provides  a  useful  framework  for  following  a 
continuous  curvature  path.  Though  the  errors  produced  following  a  curved  path  are  larger 
compared  to  those  experienced  in  linear  path  tracking,  feasibility  for  use  with  AUVs  has 
been  demonstrated. 

The  use  of  curvature  to  create  real  time  obstacle  avoidance  paths  is  possible.  A 
simple  reflexive  mine  avoidance  algorithm  has  been  developed  which  is  easy  to  apply.  But 
this  simple  reflexive  algorithm  has  limitations.  The  magnitude  and  rate  of  curvature  must 
be  limited  to  the  vehicle's  dynamic  capabilities  and  these  values  are  speed  dependent. 
When  limiting  curvature  changes  to  the  vehicle's  dynamic  capability,  avoidance  can  not 
always  be  guaranteed.  There  is  a  maximum  mine  field  density  for  the  given  vehicle's 
turning  capabilities  and  a  specified  mine  avoidance  range. 

Although  only  a  few  examples  have  been  presented,  in  a  large  number  of  simulations 
during  the  course  of  this  study,  the  combination  of  path  generation  and  nonlinear  control 
has  been  successfully  demonstrated. 
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B.  RECOMMENDATIONS 

Since  DR  is  proportional  to  K  the  use  of  curvature  to  directly  command  rudder 
orders  should  be  evaluated.  Combining  curvature  generated  from  the  avoidance  routine 
with  a  speed  controller  to  reach  the  waypoints  on  time  may  be  a  simpler  control  system. 
This  would  bypass  the  need  for  the  generation  of  a  path.  Though  using  only  curvature 
commands  may  appear  to  be  a  simpler  possibility  for  rudder  control  in  a  Mine  Warfare 
situation,  the  primary  thrust  of  this  thesis  was  to  develop  an  ability  to  follow  curved  paths. 
The  Mine  Warfare  application  was  only  used  as  an  example. 

The  addition  of  thrusters  on  the  AUV  II  this  fall  will  significantly  improve  its 
maneuvering  capability  and  allow  operation  in  a  higher  density  mine  field.  Implementation 
of  thrusters  into  this  control  architecture  should  be  attempted.  Rule  based  control  of  the 
thrusters  could  be  accomplished  easily. 

The  selection  of  the  sliding  mode  parameters  is  all  trial  and  error.  Future  work  should 
determine  a  sliding  mode  parameter  prediction  method  that  would  take  the  "guesswork” 
out  of  parameter  selection. 
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APPENDIX  A.-COMPUTER  CODE  FOR  NONLINEAR  MIMO  CONTROL 


%  PROGRAM  EQNX302M  M 

%  THIS  NON-LINEAR  CONTROL  PROGRAM  FOR  AUV  USES  3X3  MIMO  WITH 
%  CONTROLS  (RPM''2,FWD  RUDDER,  AFT  RUDDER) 

•/•  OBERVABLES  (HEADING.GLOBAL  X,  GLOBAL  Y)  AND 
%  SLIDING  MODE  CONTROL  (SIGMA=Y+LAMDA*Ydot) 

%  PATH  GENERATED  BY  AVOID2  M  AND  STORED  IN  PATH  MAT 

%  USES  RUDDER  LIMITS  OF  25  DEGREES 

clear; 

%  CONSTANT  PARAMETERS 

Weight=435; 

L  =87.625/12; 

g  =32.2; 

Mass  =Weight/g; 

Izz  =45; 

RHO  =1.94; 

CDO  =0.015; 

Umax  =2.3; 

RPMmax  =500; 
delt  =1; 

DRlim  =25/180*pi; 

ETA=[20,0,0; 

0,10,0; 

0,0,10]; 

PSI=[1;10;10]; 

LAMDA=[.  5,0,0;  %  Effectiveness  of  higher  order  terms 

0,5,0; 

0,0,5]; 

%load  path 

load  c:'data\dean\matlab\cs43 1 3\path  %  Path  to  folIow(YD,  YDdot,  YDdot2) 

N=length(YD(l,;)); 


%  Turns  per  Knot  ratio 
%  Max  prop  RPM 
%  Time  step 
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•/.  HYDRODYNAMIC  COEFFICIENTS 

Xudot  =-0.00282*0.5*RHO*L'^3; 

Xuu  =CDO*0.5*RHO*L^2; 

Xprop  *Xuu*(Uinax/RPMmaxy'2; 

Yrdot  =-0.00178*0.5*RHO*L^; 

Yr  =+0.01187*0.5*RHO*L'^3; 

Yvdot  =-0.03430*0.5*RHO*L^3; 

Yv  =-0.03896*0.5*RHO*L''2; 

Ydrb  =40.02345*0.5*RHO*L'^2; 

Ydrs  =+0.02345*0.5*RHO*L''2; 

Nrdot  =-0.00047*0.5*RHO*L'^5; 

Nvdot  =-0.00178*0.5*RHO*L'^4; 

Nr  =-0.01022*0.5*RHO*L'^4; 

Nv  =-0.00769*0.5*RHO*L^3; 

Ndrb  =+0.283  *L*  Ydrb; 

Ndrs  =-0.377*L*Ydrs; 

M=  [(Mass-Xudot), 0,0,0, 0,0; 

0,(Mass-Yvdot),0,0,0,0; 
0,0,(Izz-Nrdot),0,0,0; 

0,0,0, 1,0,0; 

0,0,0, 0,1,0, 

0,0,0,0,0,11; 

H=  [0,0,0,1,0,0; 

0,0, 0,0, 1,0; 

0,0,0,0,0,11; 

%  INITIAL  CONDITIONS 


X(;,l)  =[2;0;0;YD(1,1);YD(2,1);YD(3,1)1; 

Xdot(:,  1  )=[0;0;0;  YDdot(  1 , 1 );  YDdot(2, 1  );YDdot(3, 1  )1; 


for  i=l:N; 


G=[Xprop,  0, 

0,  Ydrb*(X(l,ir2), 
0,  Ndrb*(X(  1,1^2), 

0.  0, 

0,  0, 

0,  0. 


0; 

Ydrs*(X(l,ir2); 

Ndrs*(X(l,ir2); 


0; 

0; 


0]; 
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F(:,i)=[(Mass*X(2,i)*X(3.i)-Xuu*X(l,i)*abs(X(l.i))); 

(-Mass*X(3,i)*X(l.i)+Yv*X(2.i)*X(l,i)^-Yr*X(3,i)*X(l,i)); 

(Nv*X(2,i)*X(l.i)+Nr*X(3,i)*X(I.i)); 

X(3,i); 

X(l.i)*cos(X(4,i))-X(2.i)*sin(X(4.i)); 

X(l,i)*sin(X(4,i))+X(2.i)*cos(X(4.i))]; 


Fs=sin(X(4,i)); 

Fc=cos(X(4,i)); 


Fx=[-2*Xuu*abs(X(l.i)).  Mass*X(3,i), 

-Mass*X(3,i)+Yv*X(2.i)+Yr*X(3.i),  Yv*X(l,i), 
Nv*X(2.i)+Nr*X(3.i),  Nv*X(l,i). 

0.  0, 

Fc,  -Fs, 

Fs,  Fc. 


Mass*X(2,i),  0,  0. 

(Yr-Mass)*X(l,i),  0,  0, 

Nr*X(l,i).  0.  0, 

1,  0,  0, 


0,  -X(l,i)*Fs-X(2,i)*Fc.  0, 

0,  X(l,i)*Fc-X(2.i)*Fs,  0. 


0; 

0; 

0; 

0; 

0; 

0]; 


temp=H*X(:,i)-YD(:.i); 

temp(  1  )=nonnaI(teinp(  1 ));  %  Normalize  THET A  error  +-pi 

SIGMA(:,i)=temp+LAMDA*(H*inv(M)*(F(;,i))-YDdot(;,i)); 
DEL(:,iH»nv(LAMDA*H*inv(M)*Fx*inv(M)*G)* . . . 

(-(H+LAMDA*H*inv(M)*Fx)*inv(M)*(F(;,i))+. . . 
YDdot(:,i)+LAMDA*YDdot2(;,i)... 

-ETA*tanh(SIGMA(;.i).yPSI))); 


%  REALITY  LIMITS  ON  RPM  &  RUDDER 
if  abs(DEL(  1  ,i))>RPMmax^2 

DEL(  1  ,i)=RPMmax''2*DEL(  1  ,i)/abs(DEL(  1  ,i)); 

end; 

ifabs(DEL(2,i))>DRlim 

DEL(2,i)=DRlim*DEL(2,i)/abs(DEL(2,i)); 

end; 

ifabs(DEL(3,i))>DRlim 

DEL(3,i)=DRlim*DEL(3,i)/abs(DEL(3,i)); 

end; 


Xdot(:,i+l)=inv(M)*(F(:,i)+G*DEL(:,i)+C); 
X(:,i+l)=X(:.i)+Xdot(;,i+l)*deit; 
X(4,i+l)=nonnaI(XU  ’)); 
end; 

Xdot(:,i+l)=[]; 

X(:,i+1H]; 

finplot 
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%  PROGRAM  FINPLOT  M 
%  Plots  the  output  from  EQNX302M  M 

clg,hold  off; 

w=[0:pi/10:2*pi]; 

circ=20*[sin(w)^*cos(w)]; 

2data=DATA(:,2)+j*DATA(:.l); 

[circmesh,zdatmesh]=meshdom(circ,zdata); 

circplot’^cmesh+zdatmesh; 

!del  plot.met 

%  if  memory  in  plotter  if  lull  use  Z>=2  to  reduce  the  #  data  points  by  that  factor 
Z=2 

dels=.2  %  Incremental  distance  of  path(U*delt) 

axis('square'); 

subplot(221); 

axi^[0,length(K)*dels,-.  1,1]); 
plot([l  :Z;length(K)]'*dels,K(l  :Z:N));grid; 
title('Curvature  and  dK/dS'); 
xlabel('S');ylabel('K  &  dK/dS'); 
hold  on; 

plot([l  ;length(K)-l]'*dels,(K(2;N)-K(l  ;N.l))/dels.’-’); 
hold  off; 

subplot(222); 

%axis((-10, 140,-10, 140]); 
axis([-50,50,-50,50]); 

%axis([- 1 00, 1 00,- 1 00, 1 00]); 

plot(X(6, 1  :Z:N),X(5, 1  :Z:N),YD(3, 1  :Z:N),YD(2, 1  ;Z:N));grid; 

titleCPOSITIONCfeet)'); 

xlabelCY’);ylabel(’X); 

hold  on; 

plot([-100,100],[-100,100]); 

plot(DATA(:,2),DATA(:,l),'*’); 

for  i=l;length(circplot(l,:)) 

plot(circplot(:,i),'.'); 

end; 

hold  off; 
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subpIot(223) 

axi^[0,length(DEL(  1 ,  :))*dels,-4,4]); 
plot([  1  :N]'*dels,SIGMA(  1 ,  :)•  1 80/pi, ’-r*); 
hold  on 

plot([  1  :Z:N]**deIs,SIGMA(2, 1  ;Z  N),'-g'); 

pIot([  1  :Z:N]'*deIs,SIGMA(3, 1  ;Z;N),'-.b'); 

grid;title(’SIGMA'); 

gtextCpsi'); 

gtext(V);gtext(y); 

xlabel('S');ylabel('diff(degrees,feet)'); 

hold  off 

subplot(224) 

axi^[0,length(DEL(  1 ,  :))*dels,-30,30]); 
plot([  1  :N]'*dels,Umax/RPMmax*  sqrt(DEL(  1 , 
hold  on 

plot([  1  :Z;N]**dels,DEL(2, 1  :Z;N)*  1 80/pi,’-g*); 

plot([l  :Z:N]'*dels,DEL(3, 1  :Z;N)*  180/pi,’-.b'); 

title('DEL');xlabel('S');yIabeI('ORDER(degrees,feet/sec)');grid; 

gtextCUd*); 

gtextCDRb'); 

gtextCDRs'); 

hold  off 

meta  plot; 
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APPENDIX  B.-COMPUTER  CODE  FOR  PATH  GENERATION 


%  PROGRAM  AVOID2  M 

%  Generates  a  path  following  a  nominal  path  while  avoiding  a  mine  field, 
clear; 

hold  off; 

clg; 

load  DATA  %  Loads  current  mine  field  data  file 


axis([-100,100.-100,100]); 
plot(DATA(:.  l),DATA(:,2)/+r'); 
titleCMINE  AVOIDANCE/LOCALIZATION’); 
xlabel('X);ylabel('Y’);  %  Plot  mine  field 

hold  on; 

Xl=-50; 

Yl=-50;  %  Path  start  position 

X2=50; 

Y2=50;  %  Path  finish  position 


m=(Y2-Yl)/(X2-Xl); 

THET  Aim*atan2(  Y2-Y 1  ,X2-X  1 ); 
b=Yl.m*Xl; 

Kim=0; 


%  Slope  of  path 
%  Orientation  of  path 
%  Y  intercept  of  line 
%  Curvature  of  path 


delt=.l; 

U=2; 

dels=U*delt; 

N=800; 

L=N*dels; 

k=.l; 


%  path  increments 


%  number  of  data  points 
%  Initial  length  of  arc 
%  Manuever  sharpness  factor 


THETA(l)=pi/4; 

X(l)=-50; 

Y(l>=-50.00001; 

s(l)^; 

K(l>=le-10; 

delK(l)=0; 


%  Initial  conditions 
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for  i=l  :N,  %  Calculations  for  intercept 

s(i+ 1  )=s(i)+dels;  %  Path  position 

%Distance  to  path  calculation 

D(i)=sqrt((!n^2*X(i)-m*(Y(i)-b))^2+(Y(i)-X(i)*m-b)^2)/(m''2+ 1 ); 

if  atan2(Y(i)-Yl,X(i)-Xl)<THETAim,  %  Determines  L/R  of  line 

D(i)=-D(i); 

end; 

delKp(i)=-(3  ‘k^OCCi^KirnKS  •k^2*normal(THETA(i)-THETAim)+k^3  *D(i)); 
%  call  Sensor  routine  to  determine  if  mine  is  detected  and  orders  delK  to  avoid 


sensor  m 


delK(i)=delKp(i)+delKa(i);  %Total  delK 

%  Limit  on  delK  to  ensure  continuous  K 
if  abs(delK(i))>0.005,  delK(i)=  005*abs(delK(i))/delK(i);  end; 
K(i+I)=K(i)+deIK(i)*dels; 

if  abs(K(i+l))>0.050,  K(i+l)=.050*abs(K(i+l))/K(i+l),  end; 
THETAdot(i)=K(i)*U;  %  THETA  Rate 

delTHETA(i)=THETAdot(i)*delt;  %  Change  in  THETA 

d=dels*(sin(delTHETA(i)/2)/(deITHETA(i)/2));  %  Distance  linear  in  delt 

THET  A(i+ 1  )=THETA(i)+delTHETA(i); 
Xdot(i)=U*cos(THETA(i)+delTHETA(i)/2); 

X(i+l)=X(i)+Xdot(i)*deIt; 

Ydot(i)=U*sin(THETA(i)+delTHETA(i)/2); 

Y(i+l)=Y(i)+Ydot(i)*delt; 
plot(X(i),Y(i);*') 


%  New  THETA 
%  X  Rate 
%  New  X 
%  Y  Rate 
%  New  Y 


end 


%  Euler  estimates  of  accelerations 


Xdot2=([0,Xdot]-[Xdot,0])/delt, 

Ydot2=([0,Ydot]-[Ydot,0])/delt; 
THETAdot2=([0,THETAdot].[THETAdot,0])/deIt; 
YD=[THETA;X;Y];  %  Generate  path  data 

YDdot=[THETAdot;Xdot;Ydot]; 

YDdot2=[THETAdot2;Xdot2;Ydot2]; 

YD(:,N+1H]; 

YDdot2(:,N+lH]; 

s(N+l)=[l; 

K(N+1H]; 

save  path  YD  YDdot  YDdot2  K  %  Save  data  for  future  evaluation 

avoidplt  %  Call  plot  routine 
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%  PROGRAM  SENSOR_M  M 
%  Input:  DATA(i),i,X,Y 
%  Output;  delKa(i), 

%  Subroutine  to  determine  delK  to  avoid  mines  detecable  by  sensors 


Kscale=.0S; 

Rhsensor=50; 

Rhavoid(i)=20; 

BRGdet=[]; 

Rhdet=[]; 

count=0; 


%  Scaling  factor 
%  Range  of  detection 
%  Min  allowed  range  to  mine 
%  Initialize  routine 


%  Distance  to  all  mines 

Rh(i,:)=sqrt(((DATA(:,l)-X(i)).^2)+((DATA(;,2)-Y(i))-2)y; 


[Rhmin(i),I(i)]=min(Rh(i,:));  %  Distance  to  nearest  mine 

ifi>l; 

for  c=l  :length(DATA(:,l));  %  Find  all  mines  within  Rhdet 

if  Rh(i,c)<=Rhsensor  &  Rh(i,c)<Rh(i-l,c) 

DATA(c,3)=DATA(c,3)+ 1 ;  %  #  detects  on  each  mine 

count=count+l; 

Rhdet(count)=Rh(i,c); 

BRGdet(count)=normaI(atan2((D  AT  A(c,2)- Y  (i)),(D  AT  A(c,  1  )-X(i)))-THET  A(i)); 
end; 
end; 
end; 

ifcount>0  %  Mine  Detected 

delKr=abs((Rhsensor-Rhdet)./(Rhdet-Rhavoid(i)));  %  Range  Correction 
delKb=l-abs(BRGdet)*2/pi;  %  Linear  Bearing  Corr 

delKa(i)=-sum(delKr.*delKb.*  abs(BRGdet)./BRGdet)*Kscale;  %  delK  for  Avoidance 
else 

delKa(i)=0;  %  No  mine  detected 

end; 
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•/oPROGRAM  AVOIDPLT  M  plotting  routine  for  PROGRAM  AVOID  M 
%  Inputs:  Rhavoid,DATApC,Y),LOWLEFT.UPRIGHT,RHmin,s,K,delK 
w=[0:pi/10:2*pi];  %  Generate  avoidance  circles 

circ=20*[sin(w)+j*cos(w)]; 

2data=DATA(:,2)+j*DATA(:.  1); 

[circmesh,zdatmesh]=meshdom(circ,zdata); 

circplot=H:ircmesh+zdatmesh; 

!del  plot.met; 
hold  off; 
clg; 

axis('square'); 
axis([-50, 50,-50, 50]); 

subplot(2 11)  %  Position  plot 

plot(Y,X,[Yl,Y2],[Xl,X2],DATA(:,2),DATA(:.l).*V); 

title('PATH’);xlabel('Y');ylabel(‘X’); 

grid, 

hold  on; 

plot(circplot,'.'); 

hold  off; 

subplot(223);  %  Range  plot 

axis([0,max(s),0, 1 00]); 

plot(s,Rhmin,s,Rhavoid);title('Rhmin  &  Rhavoid'); 

xlabelCS’);ylabel(’Rh'); 

grid; 

subplot(224);  %  Curvature  plot 

axi^[0,max(s),-.l,.l]); 

plot(s,K,s,delK);title('Curvature  &  dK/dS'); 

xlabelCS');ylabel('K  &  dK/dS'); 

grid; 

meta  plot; 
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